﻿
// 2021/12/2: 增加俯仰角速度和横滚角速度。基于gnssimu-sample-v6.h (版本2020/11/11)
// 2021/12/16: 增加水平误差和垂直误差
// 2022/3/16: 增加行驶曲率
// 2022/11/2: 支持Spadas8.4
// 2022/11/14: 实现插值
// 2023/10/11: 补充部分英文注释
// 2023/11/14: 增加TAI时间类型。增加东北天向速度获取函数enuSpeed

#ifndef GNSSIMU_SAMPLE_V7_H
#define GNSSIMU_SAMPLE_V7_H

#include "spadas.h"

namespace gnssimu_sample_v7
{
	using namespace spadas;

	enum GnssImuLocationMode
	{
		GILM_NoLocation = 0, // 无位置信息
		GILM_Normal = 1, // 默认模式
		GILM_RTKFixed = 2, // RTK固定解
		GILM_RTKFloat = 3, // RTK浮动解
		GILM_RTD = 4, // RTD
		GILM_IMUOnly = 5, // 仅惯导
		GILM_Modified = 6, // 后期修正
	};

	enum GnssTimeType
	{
		GTT_UTC = 0, // UTC时间
		GTT_GPS = 1, // GPS时间
		GTT_TAI = 2, // TAI时间
	};

	struct GnssImuSample
	{
		FullTimestamp timestamp; // 时间戳

		GnssImuLocationMode locationMode; // 位置模式
		OptionalInt satelliteCount; // 卫星数量

		OptionalDouble longitude; // [°E] Longitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的经度
		OptionalDouble latitude; // [°N] Latitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的纬度
		OptionalDouble altitude; // [m] Altitude of vehicle's origin (Ground point of front end center) / 车辆坐标系原点（车前保中心地面）的海拔

		OptionalDouble antennaLongitude; // [°E] Longitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的经度
		OptionalDouble antennaLatitude; // [°N] Latitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的纬度
		OptionalDouble antennaAltitude; // [m] 天线位置海拔Altitude of device's origin (Could be antenna or rear axis center, etc.) / 设备坐标系原点（可为天线位置或后轴中心等）的海拔

		OptionalDouble horizontalError; // [m] 水平位置误差
		OptionalDouble verticalError; // [m] 垂直位置误差

		OptionalDouble speed; // [kph] 车速

		OptionalDouble orientation; // [deg -180~180] 0 as north, CCW is positive / 朝向 CCW为正 北为0
		OptionalDouble pitch; // [deg] Heading down is positive / 俯仰角 车头朝下为正
		OptionalDouble roll; // [deg] Right side heading down is positive / 横滚角 右侧朝下为正

		OptionalDouble yawRate; // [deg/s] Turning left is positive / 横摆角速度 左转为正
		OptionalDouble pitchRate; // [deg/s] Heading down is positive / 俯仰角速度 车头朝下为正
		OptionalDouble rollRate; // [deg/s] Right side heading down is positive / 横滚角速度 右侧朝下为正

		OptionalDouble ax; // [m/s2] Front side is positive / 纵向加速度 朝前为正
		OptionalDouble ay; // [m/s2] Left side is positive / 横向加速度 朝左为正
		OptionalDouble az; // [m/s2] Top side is positive / 天向加速度 朝上为正

		OptionalDouble curvature; // [1/m] Turning left is positive / 行驶曲率 左转为正

		OptionalDouble slipAngle; // [deg] Turning left is positive / 侧偏角 左转时为正
		OptionalDouble jerkX; // [m/s3] Front side is positive / 纵向急动度  朝前为正
		OptionalDouble jerkY; // [m/s3] Left side is positive / 横向急动度 朝左为正

		OptionalDouble arrivalTime; // [s] Arrival time offset (Relative to the session) / 数据到达时间戳（相对Session开始时间）
		GnssTimeType gnssTimeType; // 卫星时间类型
		Optional<TimeWithMS> gnssTime; // 卫星时间

		GnssImuSample()
		{
			locationMode = GILM_NoLocation;
			gnssTimeType = GTT_UTC;
		}

		Optional<TimeWithMS> gnssTimeUTC()
		{
			if (gnssTime.isValid())
			{
				if (gnssTimeType == GTT_UTC) return gnssTime;
				else if (gnssTimeType == GTT_GPS) return gnssTime.value() + (-18000);
				else if (gnssTimeType == GTT_TAI) return gnssTime.value() + (-37000);
			}
			return Optional<TimeWithMS>();
		}

		Optional<Vector3D> enuSpeed() // [kph]
		{
			if (speed.valid && orientation.valid && pitch.valid)
			{
				Vector3D v;
				v.z = -speed.value * math::sin(pitch.value);
				Double hSpeed = speed.value * math::cos(pitch.value);
				v.y = hSpeed * math::cos(orientation.value);
				v.x = -hSpeed * math::sin(orientation.value);
				return v;
			}
			else return Optional<Vector3D>();
		}

		static String getProtocolName(UInt channel)
		{
			if (channel == UINF) return "gnssimu-sample-v7";
			else return (String)"gnssimu-sample-v7@" + channel;
		}

		static Bool supportInterpolation()
		{
			return TRUE;
		}

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp = timestamp;
			sample.values = Array<GeneralElement>(40);
			GeneralElement *data = sample.values.data();

			data[0] = (Double)(Int)locationMode;
			data[1] = longitude;
			data[2] = latitude;
			data[3] = altitude;
			data[4] = speed;
			data[5] = orientation;
			data[6] = pitch;
			data[7] = roll;
			data[8] = yawRate;
			data[9] = ax;
			data[10] = ay;
			data[11] = az;
			data[12] = slipAngle;
			data[13] = jerkX;
			data[14] = jerkY;
			data[15] = satelliteCount.valid ? satelliteCount.value : OptionalDouble();
			data[16] = antennaLongitude;
			data[17] = antennaLatitude;
			data[18] = antennaAltitude;
			data[19] = (Double)(Int)gnssTimeType;
			data[20] = arrivalTime;
			data[21] = gnssTime.isValid() ? gnssTime.refValue().dateTime.year : OptionalDouble();
			data[22] = gnssTime.isValid() ? gnssTime.refValue().dateTime.month : OptionalDouble();
			data[23] = gnssTime.isValid() ? gnssTime.refValue().dateTime.day : OptionalDouble();
			data[24] = gnssTime.isValid() ? gnssTime.refValue().dateTime.hour : OptionalDouble();
			data[25] = gnssTime.isValid() ? gnssTime.refValue().dateTime.minute : OptionalDouble();
			data[26] = gnssTime.isValid() ? gnssTime.refValue().dateTime.second : OptionalDouble();
			data[27] = gnssTime.isValid() ? (Double)gnssTime.refValue().milliseconds : OptionalDouble();
			data[28] = pitchRate;
			data[29] = rollRate;
			data[30] = horizontalError;
			data[31] = verticalError;
			data[32] = curvature;

			sample.significantCount = 40;
			return sample;
		}

		Bool fromGeneralSample(String protocol, SessionGeneralSample sample)
		{
			timestamp = sample.timestamp;

			if (protocol == "gnssimu-sample-v7")
			{
				if (sample.values.size() != 40) return FALSE;

				locationMode = sample.values[0].valid ? (GnssImuLocationMode)(Int)sample.values[0].value : GILM_NoLocation;
				longitude = sample.values[1];
				latitude = sample.values[2];
				altitude = sample.values[3];
				speed = sample.values[4];
				orientation = sample.values[5];
				pitch = sample.values[6];
				roll = sample.values[7];
				yawRate = sample.values[8];
				ax = sample.values[9];
				ay = sample.values[10];
				az = sample.values[11];
				slipAngle = sample.values[12];
				jerkX = sample.values[13];
				jerkY = sample.values[14];
				satelliteCount = sample.values[15].valid ? (Int)sample.values[15].value : OptionalInt();
				antennaLongitude = sample.values[16];
				antennaLatitude = sample.values[17];
				antennaAltitude = sample.values[18];

				gnssTimeType = sample.values[19].valid ? (GnssTimeType)(Int)sample.values[19].value : GTT_UTC;
				arrivalTime = sample.values[20];
				if (sample.values[21].valid &&
					sample.values[22].valid &&
					sample.values[23].valid &&
					sample.values[24].valid &&
					sample.values[25].valid &&
					sample.values[26].valid &&
					sample.values[27].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[21].value,
						(UInt)sample.values[22].value,
						(UInt)sample.values[23].value,
						(UInt)sample.values[24].value,
						(UInt)sample.values[25].value,
						(UInt)sample.values[26].value,
						(UInt)sample.values[27].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				pitchRate = sample.values[28];
				rollRate = sample.values[29];
				horizontalError = sample.values[30];
				verticalError = sample.values[31];
				curvature = sample.values[32];

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v6")
			{
				if (sample.values.size() != 28) return FALSE;

				locationMode = sample.values[0].valid ? (GnssImuLocationMode)(Int)sample.values[0].value : GILM_NoLocation;
				longitude = sample.values[1];
				latitude = sample.values[2];
				altitude = sample.values[3];
				speed = sample.values[4];
				orientation = sample.values[5];
				pitch = sample.values[6];
				roll = sample.values[7];
				yawRate = sample.values[8];
				ax = sample.values[9];
				ay = sample.values[10];
				az = sample.values[11];
				slipAngle = sample.values[12];
				jerkX = sample.values[13];
				jerkY = sample.values[14];
				satelliteCount = sample.values[15].valid ? (Int)sample.values[15].value : OptionalInt();
				antennaLongitude = sample.values[16];
				antennaLatitude = sample.values[17];
				antennaAltitude = sample.values[18];

				gnssTimeType = sample.values[19].valid ? (GnssTimeType)(Int)sample.values[19].value : GTT_UTC;
				arrivalTime = sample.values[20];
				if (sample.values[21].valid &&
					sample.values[22].valid &&
					sample.values[23].valid &&
					sample.values[24].valid &&
					sample.values[25].valid &&
					sample.values[26].valid &&
					sample.values[27].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[21].value,
						(UInt)sample.values[22].value,
						(UInt)sample.values[23].value,
						(UInt)sample.values[24].value,
						(UInt)sample.values[25].value,
						(UInt)sample.values[26].value,
						(UInt)sample.values[27].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v5")
			{
				if (sample.values.size() != 19) return FALSE;

				locationMode = sample.values[0].valid ? (GnssImuLocationMode)(Int)sample.values[0].value : GILM_NoLocation;
				longitude = sample.values[1];
				latitude = sample.values[2];
				antennaAltitude = sample.values[3];
				speed = sample.values[4];
				orientation = sample.values[5];
				pitch = sample.values[6];
				roll = sample.values[7];
				yawRate = sample.values[8];
				ax = sample.values[9];
				ay = sample.values[10];
				az = sample.values[11];

				gnssTimeType = GTT_UTC;
				arrivalTime = sample.timestamp.offset;
				if (sample.values[12].valid &&
					sample.values[13].valid &&
					sample.values[14].valid &&
					sample.values[15].valid &&
					sample.values[16].valid &&
					sample.values[17].valid &&
					sample.values[18].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[12].value,
						(UInt)sample.values[13].value,
						(UInt)sample.values[14].value,
						(UInt)sample.values[15].value,
						(UInt)sample.values[16].value,
						(UInt)sample.values[17].value,
						(UInt)sample.values[18].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}
			else if (protocol == "gnssimu-sample-v4")
			{
				if (sample.values.size() != 18) return FALSE;

				longitude = sample.values[0];
				latitude = sample.values[1];
				antennaAltitude = sample.values[2];
				speed = sample.values[3];
				orientation = sample.values[4];
				pitch = sample.values[5];
				roll = sample.values[6];
				yawRate = sample.values[7];
				ax = sample.values[8];
				ay = sample.values[9];
				az = sample.values[10];

				locationMode = longitude.valid && latitude.valid ? GILM_Normal : GILM_NoLocation;

				gnssTimeType = GTT_UTC;
				arrivalTime = sample.timestamp.offset;
				if (sample.values[11].valid &&
					sample.values[12].valid &&
					sample.values[13].valid &&
					sample.values[14].valid &&
					sample.values[15].valid &&
					sample.values[16].valid &&
					sample.values[17].valid)
				{
					gnssTime = TimeWithMS((UInt)sample.values[11].value,
						(UInt)sample.values[12].value,
						(UInt)sample.values[13].value,
						(UInt)sample.values[14].value,
						(UInt)sample.values[15].value,
						(UInt)sample.values[16].value,
						(UInt)sample.values[17].value);
				}
				else
				{
					gnssTime = Optional<TimeWithMS>();
				}

				return TRUE;
			}

			return FALSE;
		}

		static GnssImuSample interpolate(GnssImuSample& s1, Double w1, GnssImuSample& s2, Double w2, FullTimestamp timestamp)
		{
			GnssImuSample output;
			output.timestamp = timestamp;

            if (s1.locationMode == s2.locationMode)
            {
                output.locationMode = s1.locationMode;
                output.longitude = s1.longitude * w1 + s2.longitude * w2;
                output.latitude = s1.latitude * w1 + s2.latitude * w2;
                output.altitude = s1.altitude * w1 + s2.altitude * w2;
                output.antennaLongitude = s1.antennaLongitude * w1 + s2.antennaLongitude * w2;
                output.antennaLatitude = s1.antennaLatitude * w1 + s2.antennaLatitude * w2;
                output.antennaAltitude = s1.antennaAltitude * w1 + s2.antennaAltitude * w2;
            }
            else
            {
                output.locationMode = w1 > w2 ? s1.locationMode : s2.locationMode;
                output.longitude = w1 > w2 ? s1.longitude : s2.longitude;
                output.latitude = w1 > w2 ? s1.latitude : s2.latitude;
                output.altitude = w1 > w2 ? s1.altitude : s2.altitude;
                output.antennaLongitude = w1 > w2 ? s1.antennaLongitude : s2.antennaLongitude;
                output.antennaLatitude = w1 > w2 ? s1.antennaLatitude : s2.antennaLatitude;
                output.antennaAltitude = w1 > w2 ? s1.antennaAltitude : s2.antennaAltitude;
            }

            output.satelliteCount = w1 > w2 ? s1.satelliteCount : s2.satelliteCount;

            output.speed = s1.speed * w1 + s2.speed * w2;
            output.orientation = interpolateAngle(s1.orientation, w1, s2.orientation, w2);
            output.pitch = s1.pitch * w1 + s2.pitch * w2;
            output.roll = s1.roll * w1 + s2.roll * w2;
            output.yawRate = s1.yawRate * w1 + s2.yawRate * w2;
            output.ax = s1.ax * w1 + s2.ax * w2;
            output.ay = s1.ay * w1 + s2.ay * w2;
            output.az = s1.az * w1 + s2.az * w2;
            output.slipAngle = s1.slipAngle * w1 + s2.slipAngle * w2;
            output.jerkX = s1.jerkX * w1 + s2.jerkX * w2;
            output.jerkY = s1.jerkY * w1 + s2.jerkY * w2;

            output.gnssTimeType = s1.gnssTimeType;
            output.arrivalTime = s1.arrivalTime * w1 + s2.arrivalTime * w2;
            if (s1.gnssTime.isValid() && s2.gnssTime.isValid())
            {
                Long diffMS = s2.gnssTime.value() - s1.gnssTime.value();
                output.gnssTime = s1.gnssTime.value() + (Long)(diffMS * w2 / (w1 + w2));
            }

            output.pitchRate = s1.pitchRate * w1 + s2.pitchRate * w2;
            output.rollRate = s1.rollRate * w1 + s2.rollRate * w2;
            output.horizontalError = s1.horizontalError * w1 + s2.horizontalError * w2;
            output.verticalError = s1.verticalError * w1 + s2.verticalError * w2;
            output.curvature = s1.curvature * w1 + s2.curvature * w2;

			return output;
		}

        static OptionalDouble interpolateAngle(OptionalDouble a1, Double w1, OptionalDouble a2, Double w2)
        {
            if (!a1.valid || !a2.valid) return OptionalDouble();

            Double x1 = math::cos(a1.value);
            Double y1 = math::sin(a1.value);
            Double x2 = math::cos(a2.value);
            Double y2 = math::sin(a2.value);

            Double xo = x1 * w1 + x2 * w2;
            Double yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return OptionalDouble();

            return math::atan(yo, xo);
        }
	};
}

#endif
